package com.evolved.automata.experimental.bots;
import java.awt.Image;
import java.awt.Toolkit;
import java.awt.image.PixelGrabber;
import java.io.*;
import java.net.Inet4Address;
import java.net.InetAddress;
import java.net.Socket;
import java.util.*;


public class SRV1SocketInterface {

	int SPort=0;
	String SIP="";
	
	int responseLength;
	byte leftMotorPower;
	byte rightMotorPower;
	short MotorDurationMilli;
	
	InputStream sistream;
	OutputStream sostream;
	Socket socket;
	int responseTimeoutMilli;
	COMMAND_STATE processState;
	
	String laserCommand;
	private static final String LowVoltageMessage="##D - low battery voltage detected";
	private static final String NormalVoltageMessage="##D - battery voltage okay";
	private static final int DefaultImageStreamDelayMilli=50;
	private String VoltageMessage="";
	private static final boolean UseAlternativeInput=true;
	
	public enum COMMAND_STATE
	{
		INITIALIZING_SOCKET,
		GETTING_STREAMS,
		GETTING_BATTERY_HEALTH,
		GETTING_JPEG_IMAGE,
		CHANGING_LASER_STATE,
		EXECUTING_MOTOR_COMMAND,
		SETTING_CAPTURE_RESOLUTION,
		GETTING_RANGE_SIMPLE,
		FINAL_STATE
	}
	
	public enum FRAMESIZE
	{
		f160x120,
		f320x240,
		f640x480,
		f1280x1020
	}
	
	private static final byte ResponseAcknowledge=35;
	private byte[] CurrentImage;
	
	String ImageSize;
	
	int width=320;
	int height=240;
	
	private static final int HALF_POWER=64;
	private static final int FULL_POWER=120;
	private static final int QUARTER_POWER=32;
	
	private static final int SHORT_STEP=100;
	private static final int ImageRequestDelayMilli=10;
	private static final int ImageRequestMaxCount=10;
	
	private int MaxImageRequestRetryCount=1;
	
	public SRV1SocketInterface(String ip,int port, int responseTimeoutMilli)
	{
		SPort=port;
		SIP=ip;
		this.responseTimeoutMilli=responseTimeoutMilli;
	}
	
	public SRV1SocketInterface()
	{
		SPort=10001;
		SIP="169.254.0.10";
		responseTimeoutMilli=2000;
	}
	public void TurnOffLasers()
	{
		laserCommand="L";
		processState=COMMAND_STATE.CHANGING_LASER_STATE;
		ExecuteStateMachine();
	}
	
	public void TurnOnLasers()
	{
		laserCommand="l";
		processState=COMMAND_STATE.CHANGING_LASER_STATE;
		ExecuteStateMachine();
	}
	
	public void RotateRightHalf()
	{
		MoveRobot(HALF_POWER,-HALF_POWER,SHORT_STEP);
	}
	
	public void RotateLeftHalf()
	{
		MoveRobot(-HALF_POWER,HALF_POWER,SHORT_STEP);
	}
	
	public void ForwardHalf()
	{
		MoveRobot(HALF_POWER,HALF_POWER,SHORT_STEP);
	}
	
	public void BackwardHalf()
	{
		MoveRobot(-HALF_POWER,-HALF_POWER,SHORT_STEP);
	}
	
	public boolean GetBatteryState()
	{
		processState=COMMAND_STATE.GETTING_BATTERY_HEALTH;
		ExecuteStateMachine();
		return (VoltageMessage.equalsIgnoreCase(NormalVoltageMessage));
		
	}
	
	public int GetWidth()
	{
		return width;
	}
	
	public int GetHeight()
	{
		return height;
	}
	
	public int[] GetCurrentImage() 
	{
		PixelGrabber pGrabber;
		int[] rawImage,rawData;
		int k=0;
		try
		{
			for (int counter=0;counter<MaxImageRequestRetryCount;counter++)
			{
				processState=COMMAND_STATE.GETTING_JPEG_IMAGE;
				ExecuteStateMachine();
				
				if ((CurrentImage!=null))
				{
					
					rawImage = new int[3*width*height];
					pGrabber = new PixelGrabber(Toolkit.getDefaultToolkit().createImage(CurrentImage),0,0,width,height,false);
					pGrabber.grabPixels();
					rawData=(int[])pGrabber.getPixels();
					for (int i=0;i<height;i++)
						for (int j=0;j<width;j++)
						{
							rawImage[k]=((rawData[i*width+j] >> 16) & 0xff);
							rawImage[k+1]= ((rawData[i*width+j] >> 8) & 0xff);
							rawImage[k+2]= ((rawData[i*width+j]) & 0xff);
							k+=3;
						}
					return rawImage;
				}
				Pause(10);
			}
		}
		catch (Exception e)
		{
			java.io.StringWriter traceText = new StringWriter();
			java.io.PrintWriter pWriter = new PrintWriter(traceText,true);
			e.printStackTrace(pWriter);
			pWriter.close();
			throw new RuntimeException(traceText.toString());
		}
		return null;
	}
	
	public void GetRange()
	{
		processState=COMMAND_STATE.GETTING_RANGE_SIMPLE;
		ExecuteStateMachine();
	}
	
	public void SetImageSize(FRAMESIZE fSize)
	{
		String iSpec;
		switch (fSize)
		{
			case f160x120:
				iSpec="a";
				break;
			case f320x240:
				iSpec="b";
				break;
			case f640x480:
				iSpec="c";
				break;
			case f1280x1020:
				iSpec="d";
				break;
			default:
				iSpec="b";
		}
		
		
		ImageSize=iSpec;
		processState=COMMAND_STATE.SETTING_CAPTURE_RESOLUTION;
		ExecuteStateMachine();
	}
	
	private boolean ExecuteStateMachine()
	{
		boolean processSuccessful=true;
		COMMAND_STATE currentState=COMMAND_STATE.INITIALIZING_SOCKET;
		byte motorCharacter; 
		byte[] output=null;
		try
		{
			motorCharacter="M".getBytes("US-ASCII")[0];
			while (currentState!=COMMAND_STATE.FINAL_STATE)
			{
				switch (currentState)
				{
					case INITIALIZING_SOCKET:
						socket=new Socket(InetAddress.getByName(SIP),SPort);
						socket.setSoTimeout(responseTimeoutMilli);
						currentState=COMMAND_STATE.GETTING_STREAMS;
						break;
					case GETTING_STREAMS:
						sistream=socket.getInputStream();
						sostream=socket.getOutputStream();
						currentState =processState;
						break;
					case CHANGING_LASER_STATE:
						ProcessFixedResponseCommand(laserCommand.getBytes("US-ASCII"),2);
						currentState=COMMAND_STATE.FINAL_STATE;
						break;
					case EXECUTING_MOTOR_COMMAND:
						ProcessFixedResponseCommand(new byte[]{motorCharacter,leftMotorPower,rightMotorPower,(byte)(MotorDurationMilli/10)},2);
						currentState=COMMAND_STATE.FINAL_STATE;
						break;
					case SETTING_CAPTURE_RESOLUTION:
						output=ProcessFixedResponseCommand(ImageSize.getBytes("US-ASCII"),2);
						Debug(new String(output,"US-ASCII"));
						
						currentState=COMMAND_STATE.FINAL_STATE;
						break;
					case GETTING_JPEG_IMAGE:
						
						processSuccessful=SetCurrentImage();
						currentState=COMMAND_STATE.FINAL_STATE;
						break;
					case GETTING_BATTERY_HEALTH:
						
						output=ProcessVariableResponseCommand("D".getBytes("US-ASCII"), 100);
						VoltageMessage=new String(output,"US-ASCII").trim();
						Debug(VoltageMessage);
						currentState=COMMAND_STATE.FINAL_STATE;
						break;
					case GETTING_RANGE_SIMPLE:
						output=ProcessVariableResponseCommand("R".getBytes("US-ASCII"), 100);
						Debug(new String(output,"US-ASCII"));
						currentState=COMMAND_STATE.FINAL_STATE;
						break;
					default:
						currentState=COMMAND_STATE.FINAL_STATE;
				}
			}
		}
		catch (Exception e)
		{
			//e.printStackTrace();
		}
		finally
		{
			try
			{
				if (sistream!=null)
					sistream.close();
				if (sostream!=null)
					sostream.close();
				if (socket!=null)
					socket.close();
			}
			catch (Exception e2)
			{
				Error(e2.toString());
				processSuccessful=false;
			}
		}
		return processSuccessful;
	}
	
	
	private boolean SetCurrentImage() throws Exception
	{
		// Send image command
		byte[] sizeResponse=null;
		CurrentImage=null;
		for (int i=0;i<ImageRequestMaxCount;i++)
		{
			SendSimpleCommand("I".getBytes("US-ASCII"));
			try
			{
				sizeResponse=ReceiveSimpleResponse(10);
				if (sizeResponse!=null)
					break;
			}
			catch (Exception e)
			{
				
			}
		}
		
		if (sizeResponse==null)
			return false;
		//Debug(new String(sizeResponse,"US-ASCII"));
		
		byte iSize=sizeResponse[5];
		SetFrameSize(iSize);
	
		int frameSize=0;
		for (int i = 6; i < 10; i++) 
        {  
            frameSize += sizeResponse[i] * Math.pow(256, i-6);
        }
		byte[] rawImage;
		Pause(DefaultImageStreamDelayMilli);
		CurrentImage=ReadFixData(frameSize);
		return true;
	}


	private void SetFrameSize(byte bRep)
	{
		int iRep=0x000000FF & bRep;
		switch (iRep)
		{
			case 49: // 1
				width=80;
				height=64;
				break;
			case 51: // 3
				width=160;
				height=120;
				break;
			case 53: // 5
				width=320;
				height=240;
				break;
			case 55: // 7
				width=640;
				height=480;
				break;
			case 57: // 9
				width=1280;
				height=1024;
				break;
		}
	}

	
	public boolean MoveRobot(int leftPower, int rightPower, int durationMilli)
	{
		leftMotorPower=(byte)leftPower;
		rightMotorPower=(byte)rightPower;
		MotorDurationMilli=(short)durationMilli;
		processState=COMMAND_STATE.EXECUTING_MOTOR_COMMAND;
		return ExecuteStateMachine();
	}
	
	

	
	private byte[] ProcessFixedResponseCommand(byte[] command, int responseLength) throws Exception
	{
		SendSimpleCommand(command);
		return ReceiveSimpleResponse(responseLength);
			
	}

	
	private void SendSimpleCommand(byte[] command) throws Exception
	{
		sostream.write(command, 0, command.length);
	}
	
	private byte[] ReceiveSimpleResponse(int responseLength) throws Exception
	{
		byte[] response=new byte[responseLength];
		int bread=0;
		bread=sistream.read(response,0,responseLength);
		if (bread==responseLength)
		{
			// Check for correct response
			String sresponse=new String(response,"US-ASCII");
			Debug(sresponse);
			return response;
		}
		return null;
	}
	
	private byte[] ReceiveSimpleResponse(int responseLength,boolean echo) throws Exception
	{
		byte[] response=new byte[responseLength];
		int bread=0;
		bread=sistream.read(response,0,responseLength);
		if (bread==responseLength)
		{
			// Check for correct response
			String sresponse=new String(response,"US-ASCII");
			if (echo)
				Debug(sresponse);
			return response;
		}
		return null;
	}
	
	private byte[] ReceiveVariableResponse(int maxLenth) throws Exception
	{
		
		boolean d1=false,d2=false;
		String dLimiter1="\r", dLimiter2="\n";
		byte[] response=new byte[maxLenth];
		int numRead=0;
		byte[] buffer = new byte[1];
		int bRead;
		while (numRead<maxLenth&&!(d1&&d2))
		{
			bRead=sistream.read(buffer, 0, 1);
			if (bRead==1)
			{
				response[numRead]=buffer[0];
				numRead++;
				if (dLimiter1.equals(new String(buffer,"US-ASCII")))
				{
					d1=true;
				}
				else if (dLimiter2.equals(new String(buffer,"US-ASCII")))
				{
					d2=true;
				}
				else
					d2=d1=false;
				if (response[numRead-1]==0)
				{
					d1=d2==true;
					break;
				}
			}
		}
		if (d1&&d2)
			return trimBytes(response, numRead);
		else
			return null;
	}
	
	private byte[] ProcessVariableResponseCommand(byte[] command,int maxResponseLength) throws Exception
	{
		
		SendSimpleCommand(command);
		return ReceiveVariableResponse(maxResponseLength);
		
	}
	
	private byte[] ReadFixData(int length) throws Exception
	{
		byte[] data= new byte[length];
		if (UseAlternativeInput)
		{
			
			byte b;
			for (int i=0;i<length;i++)
			{
				b = (byte) sistream.read();
				data[i]=b;
			}
		}
		else
		{
			sistream.read(data, 0, length);
		}
		return data;
	}
	
	
	private byte[] trimBytes(byte[] buffer,int length)
	{
		byte[] output = new byte[length];
		for (int i=0;i<length;i++)
			output[i]=buffer[i];
		return output;
	}
	
	
	private void Error(String estring)
	{
		// Disabling temporarily error output
		//System.out.println(estring);
	}
	
	private void Debug(String dstring)
	{
		// Temporarily disabling debug output
		//System.out.println(dstring);
	}

	private static void Pause(int milli)
	{
		try
		{
			Thread.currentThread().sleep(milli);
		}
		catch (Exception e)
		{
			
		}
	}
	

}
